#include <ros/ros.h>
#include <std_msgs/Empty.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PolygonStamped.h>
#include <mc_path_planner/complete_coverage_path/ccpp_planner.h>
#include <mc_map/map_base.h>

class CoveragePlannerTest{
    public:
        CoveragePlannerTest();
        ~CoveragePlannerTest();


    private:
        // 订阅地图信息
        void MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg);
        // 订阅Rviz点数据
        void ClickPointCallback(const geometry_msgs::PointStamped::Ptr& msg);
        // 数据重置
        void resetDataCallback(const std_msgs::Empty& msg);
        // 请求生成路径
        void pathGenCallback(const std_msgs::Empty& msg);
        ros::NodeHandle nh_;
        ros::Subscriber mapSub_;
        ros::Subscriber clickPointSub_;
        ros::Subscriber resetDataSub_;
        ros::Subscriber pathGenSub_;
        ros::Publisher ploygonPub_;     //多边形轮廓发布

        boost::shared_ptr<MapBase> mapHandlerPtr_;
        boost::shared_ptr<ccpp::CompleteCoverPlanner> ccppPlanner_;

        bool start_point_flag_ = false;
        geometry_msgs::Point startPoint_;         
        vector<geometry_msgs::Point> polygonArea_;    //多边形区域
};

CoveragePlannerTest::CoveragePlannerTest(){
    mapHandlerPtr_ = boost::make_shared<MapBase>();
    ccppPlanner_ = boost::make_shared<ccpp::CompleteCoverPlanner>(mapHandlerPtr_);
    mapSub_ = nh_.subscribe("/map", 10, &CoveragePlannerTest::MapCallback, this);
    clickPointSub_ = nh_.subscribe("/clicked_point", 1, &CoveragePlannerTest::ClickPointCallback, this);
    resetDataSub_ = nh_.subscribe("/test_reset_data", 1, &CoveragePlannerTest::resetDataCallback, this);
    pathGenSub_ = nh_.subscribe("/test_path_gen", 1, &CoveragePlannerTest::pathGenCallback, this);
    ploygonPub_ = nh_.advertise<geometry_msgs::PolygonStamped>("/coverage_polygon", 1, true);
    ROS_INFO("TEST......");
}

CoveragePlannerTest::~CoveragePlannerTest(){

}

// 订阅地图信息
void CoveragePlannerTest::MapCallback(const nav_msgs::OccupancyGrid::Ptr& msg){
    ROS_INFO_STREAM("[MapCallback] Receive a map. width=" << msg->info.width 
        << ", height=" << msg->info.height
        << ", resolution=" << msg->info.resolution
    );
    mapHandlerPtr_->setOccupancyGrid(msg);

    if(!ccppPlanner_) return;
    // ccppPlanner_->InitMap();

    // // TEST
    // startPoint_.x = 2.20429;
    // startPoint_.y = 0.290001;
    // geometry_msgs::Point point;
    // point.x = -0.729209;
    // point.y = 3.17295;
    // polygonArea_.emplace_back(point);
    // point.x = 5.48348;
    // point.y = 2.9974;
    // polygonArea_.emplace_back(point);
    // point.x = 6.29341;
    // point.y = -2.22036;
    // polygonArea_.emplace_back(point);
    // point.x = 2.90659;
    // point.y = -3.97095;
    // polygonArea_.emplace_back(point);
    // point.x = -0.9832;
    // point.y = -1.80912;
    // polygonArea_.emplace_back(point);
    // ccpp::CoveragePath path;
    // // ccppPlanner_->genPathWithPolygon(startPoint_, polygonArea_, ccpp::PathType::EDGE_PATH, path);    
    // ccppPlanner_->genPathWithPolygon(startPoint_, polygonArea_, ccpp::PathType::SPRIAL_AND_ARC_SHAPED_PATH, path); 
}

// 订阅Rviz点数据
void CoveragePlannerTest::ClickPointCallback(const geometry_msgs::PointStamped::Ptr& msg){
    ROS_INFO_STREAM("[ClickPointCallback] Receive a point. x=" << msg->point.x
        << ", y=" <<  msg->point.y
    );
    if(!start_point_flag_){
        start_point_flag_ = true;
        ROS_INFO("SET start point.");
        startPoint_ = msg->point;
        return;
    }
    polygonArea_.emplace_back(msg->point);
    ROS_INFO_STREAM("SET Polygon point. size: " << polygonArea_.size());

    // 发布轮廓信息
    geometry_msgs::PolygonStamped polygon_msg;
    polygon_msg.header = msg->header;
    for(auto point : polygonArea_){
        geometry_msgs::Point32 point_32;
        point_32.x = point.x;
        point_32.y = point.y;
        polygon_msg.polygon.points.emplace_back(point_32);
    }
    ploygonPub_.publish(polygon_msg);
}

// 数据重置
void CoveragePlannerTest::resetDataCallback(const std_msgs::Empty& msg){
    ROS_INFO("数据重置");
    start_point_flag_ = false;
    polygonArea_.clear();
}

// 请求生成路径
void CoveragePlannerTest::pathGenCallback(const std_msgs::Empty& msg){
    if(!start_point_flag_){
        ROS_WARN_STREAM("请设置起点!");
        return;
    }
    if(polygonArea_.size() < 3){
        ROS_WARN_STREAM("多边形点集须大于3个!");
        return;
    }

    if(ccppPlanner_){
        ccpp::CoveragePath path;
        ccppPlanner_->genPathWithPolygon(startPoint_, polygonArea_, ccpp::PathType::SPRIAL_AND_ARC_SHAPED_PATH, path);
    }
}

int main(int argc, char** argv){
    FLAGS_alsologtostderr = true;           // 日志同时输出到stderr
    FLAGS_max_log_size = 1;                 // 单个日志文件大小上限（MB）, 如果设置为0将默认为1
	FLAGS_stop_logging_if_full_disk = true; // 当磁盘写满时，停止输出
    FLAGS_minloglevel = google::INFO;       // 只记录大于{}等级的日志
    // FLAGS_log_dir = "/home/xxty/user_config/logs";
    google::InitGoogleLogging(argv[0]);

    // 捕获系统信号
    google::InstallFailureSignalHandler();

    ros::init(argc, argv, "test_coverage_planner");
    setlocale(LC_ALL,"");   //ROS_INFO 中文显示, 需要添加头文件#include <sstream>
    ROS_INFO("test_coverage_planner start...");

    CoveragePlannerTest test_coverage_planner;
    ros::spin();

    google::ShutdownGoogleLogging();

    return 0;
}